#include <TimerOne.h>

// shared code
// ================================================
// BLDC common 2021-05-12
// ================================================
#define INH_A  2
#define IN_A   3
#define INH_B  4
#define IN_B   5
#define INH_C  6
#define IN_C   7
#define HALL_A 8
#define HALL_B 10
#define HALL_C 11


const int INH[] = {INH_A, INH_B, INH_C};
const int IN[]  = {IN_A, IN_B, IN_C};
const int HALL[] = {HALL_A, HALL_B, HALL_C};


const byte steps[6][6] = {  // forward mapping
  // INH_A, IN_A, INH_B, IN_B, INH_C, IN_C
  {1,1,1,0,0,0},  // AB
  {1,1,0,0,1,0},  // AC
  {0,0,1,1,1,0},  // BC
  {1,0,1,1,0,0},  // BA
  {1,0,0,0,1,1},  // CA
  {0,0,1,0,1,1}  // CB
};


const byte halls[6][3] = {
  {0,0,1},  
  {0,1,1},
  {0,1,0},
  {1,1,0},
  {1,0,0},
  {1,0,1}
};


void InitBLDC()
{
  for (int i=0; i<3; i++){
    pinMode(INH[i], OUTPUT);
    digitalWrite(INH[i], LOW);
    pinMode(IN[i], OUTPUT);
    digitalWrite(IN[i], LOW);
    pinMode(HALL[i], INPUT_PULLUP);
  }
}


void BLDCSet(const byte inh[], const byte in[]){
  for (int i=0; i<3; i++){
    digitalWrite(INH[i], inh[i]);
    digitalWrite(IN[i], in[i]);
  }
}


int NextStep(byte inh[], byte in[], int dir=1) {
  int idx = -1;
  byte a,b,c;
  
  a = digitalRead(HALL_A);
  b = digitalRead(HALL_B);
  c = digitalRead(HALL_C);
  for (int i=0; i<6; i++) {
    if (   a == halls[i][0]
        && b == halls[i][1]
        && c == halls[i][2]) {
      idx = i;
      break;
    }
  }

  if (idx<0 || idx>5) {return -1;}  // no match

  if (dir < 0)  {idx = (idx + 3) % 6;}
  
  // copy state of current
  for (int i=0; i<3; i++){
    inh[i] = steps[idx][2*i];
    in[i] = steps[idx][2*i+1];
  }

  return idx;
}


void PrintHallStep(const byte inh[], const byte in[]){
  Serial.print("s&h: ");
  for (int i=0; i<3; i++){
    Serial.print(inh[i]); Serial.print(in[i]);
    Serial.print(" ");
  }
  
  for (int i=0; i<3; i++){
    Serial.print(digitalRead(HALL[i]));
  }
  Serial.println();
}


void shine() {
  for (int i=0; i<3; i++) {digitalWrite(INH[i], LOW);}
  for (int i=0; i<30; i++){
    digitalWrite(13, HIGH); delay(100);
    digitalWrite(13, LOW); delay(100);
  }
}


void setup() {
  Serial.begin(115200);
  pinMode(9, OUTPUT); digitalWrite(9, HIGH);
  Timer1.initialize(200);  // us
  Timer1.pwm(9, 30);  // 0 to 1023
  InitBLDC();
}


byte inh[3], in[3];
long lastTime = millis();
long thisTime = lastTime;
const unsigned int interval = 2000;

int last_step = 0;
int count = 0;
int dir = 1;

void loop() {
  thisTime = millis();
  if ((thisTime - lastTime) > interval){
    lastTime = thisTime;
//    NextStep(inh, in);
//    BLDCSet_NoPWM(inh, in);

      Serial.println(count);
      count = 0;
      dir = dir == 1 ? -1 : 1;
  }
//  
//  PrintHallStep(inh, in);
//  delay(300);

 int ret = NextStep(inh, in, dir);
 if (ret < 0) {shine();}
 BLDCSet(inh, in);

 if (ret != last_step) {
  count += (abs(ret-last_step));
//  if (abs(ret - last_step) != 1) {
//    Serial.print(ret); Serial.print(" "); 
//    Serial.print(last_step); Serial.print(" ");
//    Serial.println(ret - last_step);
//  }
  last_step = ret;
 }
}
